#include "Perpendicular.h"

namespace common {

Perpendicular::Perpendicular(int id, Point *k, Point *l, Vector *m, Vector *n):common::Constraint(id,1,PERPENDICULAR),vm(NULL),vn(NULL){

	setK(k);
	setL(l);
	setM(NULL);
	setN(NULL);

	if(dynamic_cast<Point*>(m) && dynamic_cast<Point*>(n)){
		setM(dynamic_cast<Point*>(m));
		setN(dynamic_cast<Point*>(n));
	}else {
		setVectorM(m);
		setVectorN(n);
	}
}

void Perpendicular::setVectorM(Vector* m){
	this->vm = m;

}
void Perpendicular::setVectorN(Vector* n){
	this->vn = n;
}
Vector* Perpendicular::getVectorM(void){
	return vm;
}
Vector* Perpendicular::getVectorN(void){
	return vn;
}

ublasVector Perpendicular::getValue() throw(MyException){
	// (L-K)'*(N-M)
	ublasVector v(1);
	if(getVectorM()==NULL && getVectorN()==NULL){
		v(0) = ((*getL())-(*getK()))*((*getN())-(*getM()));
	}else {
		v(0) = ((*getL())-(*getK()))*((*getVectorN())-(*getVectorM()));
	}
	return v;
}

ublasMatrix Perpendicular::getJacobianOnPoint(Point *p) throw(MyException) {

	Vector lk_((*getL())-(*getK()));
	ublasVector lk(2);
	lk << lk_;

	ublasMatrix w(1,2);

	if(getVectorM()==NULL && getVectorN()==NULL){

		Vector nm_((*getN())-(*getM()));
		ublasVector nm(2);
		nm << nm_;
		if(k==p){
			row(w,0) = -nm;
		}
		else if(l==p){
			row(w,0) = nm;
		}
		else if(m==p){
			row(w,0) = -lk;
		}
		else if(n==p){
			row(w,0) = lk;
		}
	}else {
		Vector nm_((*getVectorN())-(*getVectorM()));
		ublasVector nm(2);
		nm << nm_;
		if(k==p){
			row(w,0) = -nm;
		}
		else if(l==p){
			row(w,0) = nm;
		}
		else{  // (m==p) ||  n==p
			row(w,0) = ublasVectorZero(2);
		}
	}
	return w;
}
ublasMatrix Perpendicular::getHessian(Point* s, Point* r) throw(MyException){

	ublasMatrix out(ublasMatrixZero(2,2));
	ublasMatrixIdentity I(2,2);
	ublasMatrix mI(-I);

	if(k==s && m==r){
		out = I;
	}else if(k==s && n==r){
		out =mI;
	}else if(l==s && m==r){
		out =mI;
	}else if(l==s && n==r){
		out = I;
	}else if(m==s && k==r){
		out = I;
	}else if(m==s && l==r){
		out = mI;
	}else if(n==s && k==r){
		out = mI;
	}else if(n==s && l==r){
		out = I;
	}else {
		return ublasMatrixZero(2,2);
	}
	return out;
}
bool Perpendicular::isHessian() const{
	return true;
}

std::string Perpendicular::toString(void){
	std::stringstream o;
	if(getVectorM()!=NULL && getVectorN()!=NULL){
		o << "Constraint-" << GeometricConstraintTypeNames[getType()] << " "<< getId() <<"- [" ;
		if(getK()!=NULL) o <<" p" << getK()->getId() << "-("<< getK()->getX() <<"," <<  getK()->getY() << ")"<< " ,";
		if(getL()!=NULL) o <<" p" << getL()->getId() << "-("<< getL()->getX() <<"," <<  getL()->getY() << ")"<< " ,";
		if(getVectorM()!=NULL) o <<" v" << "-("<< getVectorM()->getX() <<"," <<  getVectorM()->getY() << ")"<< " ,";
		if(getVectorN()!=NULL) o <<" v" << "-("<< getVectorN()->getX() <<"," <<  getVectorN()->getY() << ")"<< "";

		o << "]";
	}else {
		o << Constraint::toString();
	}
	return o.str();
}
Perpendicular::~Perpendicular() {
	if(getVectorM()!=NULL && getVectorN()!=NULL){
		delete vm;
		delete vn;

	}
}

}

